////////////////////////////////////////////////////////////
// Headers
////////////////////////////////////////////////////////////
#include "Line.hpp"

#include <SFML/OpenGL.hpp>

#include "../Math.hpp"

#ifdef _DEBUG
#include <SFML/Graphics.hpp>
#endif

#include <SmegTools/Debug/DebugNew.hpp>

namespace smeg {
    namespace math {
        
////////////////////////////////////////////////////////////
/// Constructor
////////////////////////////////////////////////////////////
Line::Line( const sf::Vector2f& _Extent1, 
            const sf::Vector2f& _Extent2 ) :
    BoundingVolume( eLine )
{
    Setup( _Extent1, _Extent2 );
}
        
////////////////////////////////////////////////////////////
/// Default Constructor
////////////////////////////////////////////////////////////
Line::Line( const Line& _Copy ) :
    BoundingVolume( _Copy )
{
    m_Extents[0] = _Copy.m_Extents[0];
    m_Extents[1] = _Copy.m_Extents[1];
}

////////////////////////////////////////////////////////////
/// Destructor
////////////////////////////////////////////////////////////
Line::~Line(void)
{
}

////////////////////////////////////////////////////////////
/// Setup the Line
////////////////////////////////////////////////////////////
void Line::Setup( const sf::Vector2f& _Extent1, const sf::Vector2f& _Extent2 ) {
    m_Extents[0] = _Extent1;
    m_Extents[1] = _Extent2;
    
    ComputeLine();
}

////////////////////////////////////////////////////////////
/// Does the Bounding Volume overlaps with another volume
////////////////////////////////////////////////////////////
bool Line::Overlaps( const BoundingBox& _Other ) const {
    return BoundingVolume::Overlaps( _Other, *this );
}

bool Line::Overlaps( const Circle& _Other ) const {
    return BoundingVolume::Overlaps( _Other, *this );
}

// Based on http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect
// Find the vector R and S that goes from m_Extents[0] to m_Extents[1] for this and _Other
//
// Let's put this as P and _Other as Q
// If the lines overlaps we can find t and u such that :
// P + tR = Q + uS
//
// Cross both side by :
// R                                    | S
// ( P + tR ) x R = ( Q + uS ) x R      | ( P + tR ) x S = ( Q + uS ) x S
//
// Since R x R = 0 and S x S = 0
// u = ( P - Q ) x R / ( S x R )        | t = ( Q - P ) x S / ( R x S )
//
// Since R x S = - S x R
// u = ( Q - P ) x R / ( R x S )        | t = ( Q - P ) x S / ( R x S )
//
// Now we have 5 cases :
// 1.   If R x S = 0 and ( Q - P ) x R = 0 => The lines are collinear.
// 1.a. If either 
//      0 <= ( Q - P ) . R <= R . R
//          or
//      0 <= ( P - Q ) . S <= S . S
//      Then the two lines are overlapping.
// 1.b. If neither 
//      0 <= ( Q - P ) . R <= R . R
//          nor
//      0 <= ( P - Q ) . S <= S . S
//      Then the two lines are disjoint.
// 2.   If R x S = 0 and ( Q - P ) x R != 0 => The lines are parallel.
// 3.   If R x S != 0 and 0 <= t <= 1 and 0 <= u <= 1 => The lines overlaps at P + tR = Q + uS 
// 4.   The two lines are not parallel but do not overlap

bool Line::Overlaps( const Line& _Other ) const {
    return Overlaps( _Other.m_Extents[0], _Other.m_Extents[1] );
}

bool Line::Overlaps( const sf::Vector2f& _Extent1, const sf::Vector2f& _Extent2 ) const {
    sf::Vector2f R( m_Extents[1] - m_Extents[0] ),
        S( _Extent2 - _Extent1  );
    
    sf::Vector2f PQ = _Extent1 - m_Extents[0],
        QP = m_Extents[0] - _Extent1;
    float RcrossS =  math::Cross( R, S ),
        PQcrossR = math::Cross( PQ, R ),
        PQcrossS = math::Cross( PQ, S );

    if ( IsZeroByEpsilon( RcrossS ) ) {
        float PQdotR = math::Dot( PQ, R ),
            QPdotS = math::Dot( QP, S );
        return IsZeroByEpsilon( PQcrossR ) && 
            ( 0.0f <= PQdotR && PQdotR <= math::Dot( R, R ) || 
                0.0f <= QPdotS && QPdotS <= math::Dot( S, S ) );
    } else {
        float u = PQcrossR / RcrossS,
            t = PQcrossS / RcrossS;
        return ( 0.0f <= u && u <= 1.0f &&
            0.0f <= t && t <= 1.0f );
    }
}

bool Line::Intersects( const Line& _Other, sf::Vector2f& _IntersectionPoint ) const {
    return Intersects( _Other.m_Extents[0], _Other.m_Extents[1], _IntersectionPoint );
}

bool Line::Intersects( const sf::Vector2f& _Extent1, const sf::Vector2f& _Extent2, sf::Vector2f& _IntersectionPoint ) const {
    sf::Vector2f R( m_Extents[1] - m_Extents[0] ),
        S( _Extent2 - _Extent1 );
    
    sf::Vector2f PQ = _Extent1 - m_Extents[0],
        QP = m_Extents[0] - _Extent1;
    float RcrossS =  math::Cross( R, S ),
        PQcrossR = math::Cross( PQ, R ),
        PQcrossS = math::Cross( PQ, S );

    if ( IsZeroByEpsilon( RcrossS ) ) {
        float PQdotR = math::Dot( PQ, R ),
            QPdotS = math::Dot( QP, S );
        return IsZeroByEpsilon( PQcrossR ) && 
            ( 0.0f <= PQdotR && PQdotR <= math::Dot( R, R ) || 
                0.0f <= QPdotS && QPdotS <= math::Dot( S, S ) );
    } else {
        float u = PQcrossR / RcrossS,
            t = PQcrossS / RcrossS;
        if ( 0.0f <= u && u <= 1.0f &&
            0.0f <= t && t <= 1.0f ) {
            _IntersectionPoint = m_Extents[0] + t * R;

            return true;
        } else {
            return false;
        }
    }
}

////////////////////////////////////////////////////////////
/// Does the Bounding Volume contains a point
////////////////////////////////////////////////////////////
bool Line::Contains( const sf::Vector2f& _Point ) const {
    // Put the point relative to the center
    sf::Vector2f FirstExtentToPoint = _Point - m_Extents[0];
    float fPointSlope = FirstExtentToPoint.y / FirstExtentToPoint.x;

    // The point is in the line if :
    // - the dot product of Norm( FirstExtentToPoint ) and Norm( m_Extents[1] - m_Extents[0] ) is around 1
    // - the length of the point from the first extent is less or equal to the length of the line
    return ( math::IsEqualByEpsilon( math::Dot( math::NormalizedCopy( FirstExtentToPoint ), math::NormalizedCopy( m_Size ) ), 1.0f ) &&
        math::SquaredLength( FirstExtentToPoint ) <= math::SquaredLength( m_Size ) );
}

////////////////////////////////////////////////////////////
/// Compute the center and the component of the line
////////////////////////////////////////////////////////////
void Line::ComputeLine() {
    m_Size      = m_Extents[1] - m_Extents[0];
    m_HalfSize  = m_Size * 0.5f;

    m_Center    = m_Extents[0] + m_HalfSize;
}

/*********************** Accessors ************************/
void Line::SetCenter( float _fX, float _fY ) {
    MoveCenter( _fX - m_Center.x, _fY - m_Center.y );
}

void Line::MoveCenter( float _fX, float _fY ) {
    BoundingVolume::MoveCenter( _fX, _fY );
    
    m_Extents[0].x += _fX;
    m_Extents[0].y += _fY;

    m_Extents[1].x += _fX;
    m_Extents[1].y += _fY;
}

float Line::GetPseudoRadius() const {
    return math::Length( m_Size );
}

#ifdef _DEBUG
void Line::DebugRender( sf::RenderWindow* _pRenderWindow, const sf::Color& _Color, bool _bFull ) const {
    sf::Vertex LineVertex[2] = {
                                sf::Vertex( m_Extents[0], _Color ),
                                sf::Vertex( m_Extents[1], _Color )
                            };

    _pRenderWindow->draw( LineVertex, 2, sf::Lines );
}
#endif // _DEBUG

    } // namespace math
} // namespace smeg